"""
来自论文[1] 许利恒.基于椭圆特征的陨石坑检测与识别方法研究[D].北京:北京航空航天大学,2022

按照论文的方法复现，关于从四点金字塔拓展至整幅陨石坑的算法，这里使用了姿态解算的结果作为重投影平差的依据，不再使用导航坑对的不变量计算，增加了对陨石坑的筛选算法
"""

import numpy as np
from .base import BinadFastPyramid
from utils.ellipse import center_ellipse


class ChenFastPyramid(BinadFastPyramid):

    def binad_descriptor(self, C1, C2, *args, factor=0.3, **kwargs):
        """
        本函数需要并行化计算
        Arguments:
            param1 (np.ndarray) : ellipse parameters for (x^2, xy, y^2, x, y, 1), 6 x N array
            param2 (np.ndarray) : ellipse parameters for (x^2, xy, y^2, x, y, 1), 6 x N array
        """
        # 直接除以行列式是不对滴，参见文献
        # [1] Gros P. , Quan L. .Projective invariants for Vision[R].1992:47
        # from Xuliheng' methon
        if len(C1.shape) == 2:
            C1 = C1[None]
        if len(C2.shape) == 2:
            C2 = C2[None]
        assert C1.shape == C2.shape
        cnt1 = np.pad(
            center_ellipse(C1), ((0, 1), (0, 0)), "constant", constant_values=1
        )
        cnt2 = np.pad(
            center_ellipse(C2), ((0, 1), (0, 0)), "constant", constant_values=1
        )

        I1 = np.einsum("nij,jn,in->n", C2, cnt1, cnt1) / np.einsum(
            "nij,jn,in->n", C2, cnt2, cnt2
        )
        I2 = np.einsum("nij,jn,in->n", C1, cnt2, cnt2) / np.einsum(
            "nij,jn,in->n", C1, cnt1, cnt1
        )
        I = np.array((I1, I2))
        return I, factor * np.abs(I)
